/**
 * @file ros_adapter.cc
 * @author HKUST Aerial Robotics Group
 * @brief
 * @version 0.1
 * @date 2019-03-18
 *
 * @copyright Copyright (c) 2019
 */
#include "phy_simulator/ros_adapter.h"
#include <std_msgs/msg/string.hpp>

namespace phy_simulator {

RosAdapter::RosAdapter() {}

RosAdapter::RosAdapter(std::shared_ptr<rclcpp::Node> node) : node_(node) {
  // arena_info_pub_ = node_->create_publisher<vehicle_msgs::msg::ArenaInfo>("arena_info", 10);
  arena_info_static_pub_ = node_->create_publisher<std_msgs::msg::String>("arena_info_static", 10);
  arena_info_dynamic_pub_ = node_->create_publisher<std_msgs::msg::String>("arena_info_dynamic", 10);
}

void RosAdapter::PublishDataWithStamp(const rclcpp::Time& stamp) {
  if (p_phy_sim_) {
    // This would need vehicle_msgs::ArenaInfo message conversion
    // For now, publish static and dynamic separately
    auto msg = std_msgs::msg::String();
    msg.data = "Arena info data";
    // arena_info_pub_->publish(msg);  // When ArenaInfo message is available
  }
}

void RosAdapter::PublishStaticDataWithStamp(const rclcpp::Time& stamp) {
  if (p_phy_sim_) {
    // Use encoder to convert data - need to update encoder for ROS2
    auto msg = std_msgs::msg::String();
    msg.data = "Arena static info data - lane_net and obstacles";
    arena_info_static_pub_->publish(msg);
  }
}

void RosAdapter::PublishDynamicDataWithStamp(const rclcpp::Time& stamp) {
  if (p_phy_sim_) {
    // Use encoder to convert data - need to update encoder for ROS2
    auto msg = std_msgs::msg::String();
    msg.data = "Arena dynamic info data - vehicles";
    arena_info_dynamic_pub_->publish(msg);
  }
}


}  // namespace phy_simulator